#ifndef MY_DWA_PLANNER_H
#define MY_DWA_PLANNER_H

#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_local_planner.h>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/world_model.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf2_ros/buffer.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <dynamic_reconfigure/server.h>
#include <my_dwa_planner/DWAPlannerConfig.h>

namespace my_dwa_planner {

class DWAPlanner : public nav_core::BaseLocalPlanner {
public:
    DWAPlanner();
    ~DWAPlanner();

    void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) override;
    bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) override;
    bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) override;
    bool isGoalReached() override;

private:
    base_local_planner::Trajectory findBestPath(
        const geometry_msgs::PoseStamped& robot_pose,
        const nav_msgs::Odometry& robot_vel,
        geometry_msgs::PoseStamped& best_pose);
    
    double scoreTrajectory(const base_local_planner::Trajectory& traj);
    double headingDiff(double x, double y, double pt_x, double pt_y);
    
    void reconfigureCB(DWAPlannerConfig &config, uint32_t level);
    
    bool transformGlobalPlan(const tf2_ros::Buffer& tf, 
                           const std::vector<geometry_msgs::PoseStamped>& global_plan,
                           const costmap_2d::Costmap2DROS& costmap,
                           const std::string& global_frame,
                           std::vector<geometry_msgs::PoseStamped>& transformed_plan);
    
    ros::Publisher traj_pub_;
    ros::Publisher best_traj_pub_;
    
    // Configuration parameters
    double max_vel_x_, min_vel_x_;
    double max_vel_y_, min_vel_y_;
    double max_vel_th_, min_vel_th_;
    double acc_lim_x_, acc_lim_y_, acc_lim_th_;
    double sim_time_, sim_granularity_;
    double angular_sim_granularity_;
    double pdist_scale_, gdist_scale_, occdist_scale_;
    double stop_time_buffer_;
    double oscillation_reset_dist_;
    double heading_lookahead_;
    double dwa_xy_tolerance_, dwa_yaw_tolerance_;
    
    std::vector<geometry_msgs::PoseStamped> global_plan_;
    costmap_2d::Costmap2DROS* costmap_ros_;
    tf2_ros::Buffer* tf_;
    base_local_planner::WorldModel* world_model_;
    
    dynamic_reconfigure::Server<DWAPlannerConfig> *dsrv_;
    bool goal_reached_;
    bool initialized_;
};

} // namespace my_dwa_planner

#endif // MY_DWA_PLANNER_H